#include <cassert>
#include "curling/stone.hpp"

namespace curling {

stone::stone( stone_color color )
: _color(color),
  _position(0.0, 0.0),
  _velocity(0.0, 0.0),
  _rotation(0.0),
  _rotational_velocity(0.0),
  _in_play(false),
  _played(false),
  _tagged_as_point(false)
{
}

stone::~stone()
{
}

double stone::mass() const
{
  return default_stone::mass;
}

double stone::radius() const
{
  return default_stone::radius;
}

double stone::height() const
{
  return default_stone::height;
}

stone_color stone::color() const
{
  return _color;
}

vector2d stone::position() const
{
  return _position;
}

vector2d stone::velocity() const
{
  return _velocity;
}

double stone::rotation() const
{
  return _rotation;
}

double stone::rotational_velocity() const
{
  return _rotational_velocity;
}

void stone::reposition( vector2d new_position, double new_rotation )
{
  _position = new_position;
  _velocity = 0.0;
  _rotation = new_rotation;
  _rotational_velocity = 0.0;
}

void stone::accellerate( vector2d add_velocity, double add_rotational_velocity )
{
  _velocity += add_velocity;
  _rotational_velocity += add_rotational_velocity;
}

bool stone::moving() const
{
  return (_velocity.magnitude() > 0.01) or (std::fabs(_rotational_velocity) > 0.01);
}

void stone::save_position()
{
  _saved_position = _position;
}

void stone::restore_position()
{
  _position = _saved_position;
}

bool stone::in_play() const
{
  return _in_play;
}

bool stone::played() const
{
  return _played;
}

void stone::put_into_play()
{
  _in_play = true;
  _played = true;
  _tagged_as_point = false;
}

void stone::remove_from_play()
{
  _in_play = false;
  _tagged_as_point = false;
  _velocity = vector2d(0.0, 0.0);
  _rotational_velocity = 0.0;
}

bool stone::tagged_as_point() const
{
  return _tagged_as_point;
}

void stone::tag_as_point()
{
  _tagged_as_point = true;
}

void stone::untag_as_point()
{
  _tagged_as_point = false;
}

void stone::reset()
{
  reposition(vector2d(0.0, 0.0), 0.0);
  _in_play = false;
  _played = false;
  _tagged_as_point = false;
}

void stone::simulate( double dt )
{
  _position += _velocity*dt;
  _rotation += _rotational_velocity*dt;
  
  vector2d force_friction = 0.0;
  vector2d force_curl = 0.0;
  
  // calculate forces acting on the stone
  if (_velocity.magnitude() > 0.01)
  {
    force_friction = backward() * friction_coefficient_slide() * mass() * 9.82;
    force_curl = right() * friction_coefficient_curl() * mass() * 9.82;
  }
  else
  {
    force_curl = vector2d(-1.0, 0.0) * friction_coefficient_curl() * mass() * 9.82;
  }
  
  // calculate new angular velocity by subtracting curling force energy
  if (std::fabs(_rotational_velocity) > 0.001)
  {
    const vector2d accelleration_curl = force_curl / mass();
    const vector2d velocity_curl = accelleration_curl * dt;
    const double energy_curl = 0.5*mass()*std::pow(velocity_curl.magnitude(), 2.0);
    const double energy_rot = 0.25*mass()*std::pow(radius(), 2.0)*std::pow(_rotational_velocity, 2.0);
    double energy_rot_prim = energy_rot - energy_curl;
    if (energy_rot_prim < 0.001)
    {
      energy_rot_prim = 0.0;
    }
    double sign = (_rotational_velocity >= 0.0 ? 1.0 : -1.0);
    _rotational_velocity = sign*(2.0/radius())*std::sqrt(energy_rot_prim / mass());
  }
  else
  {
    _rotational_velocity = 0.0;
  }
  
  // calculate new velocity
  if (_velocity.magnitude() > 0.01)
  {
    const vector2d force_total = force_friction + force_curl;
    const vector2d accelleration_total = force_total / mass();
    _velocity += accelleration_total * dt;
  }
  else
  {
    _velocity = 0.0;
  }
}

vector2d stone::forward() const
{
  vector2d dir = velocity();
  dir.normalize();
  return dir;
}

vector2d stone::backward() const
{
  return forward()*-1.0;
}

vector2d stone::left() const
{
  return right()*-1.0;
}

vector2d stone::right() const
{
  using extvec::vector3d;
  
  vector2d fwd = forward();
  vector3d tmp = cross(vector3d(0.0, 0.0, 1.0), vector3d(fwd.x(), fwd.y(), 0.0));
  tmp.normalize();
  return vector2d(tmp.x(), tmp.y());
}

double stone::friction_coefficient_slide() const
{
  if (_velocity.magnitude() < 0.01)
  {
    return 0.1;
  }
  else
  {
    return 0.0168;
  }
}

double stone::friction_coefficient_curl() const
{
  if (std::fabs(_rotational_velocity) < 0.001)
  {
    return 0.0;
  }
  
  if (_velocity.magnitude() < 0.01)
  {
    return 0.1;
  }
  else
  {
    double scaled = _velocity.magnitude() / 10.0;
    if (scaled > 1.0) scaled = 1.0;
    const double min_friction = 0.00001;
    const double max_friction = 0.005;
    const double source_friction = min_friction + ((1.0-scaled)*max_friction);
    return source_friction * _rotational_velocity;
  }
}

}